package robot;

import javax.vecmath.Point2d;
import javax.vecmath.Vector3d;

import pathAlgorithms.PathAlgorithm;
import program.Main;
import program.MovingObjects;
import program.RoboMap;

/**
 * Robot using D*
 * @date 15 maj 2012
 * @version 1.0
 */
public class DStarRobot extends Robot {
	private PathAlgorithm pathfinder;
	private Point2d[] path;
	private Point2d goal;

	public DStarRobot(Vector3d position, String name, PathAlgorithm pathfinder,
			Point2d goal, MovingObjects mo) {
		super(position, name, mo);
		this.pathfinder = pathfinder;
		this.goal = goal;
		roboLogger.path.add(getCurrentPosition());
	}
	
	public DStarRobot(Vector3d position, String name, PathAlgorithm pathfinder,
			Point2d goal, MovingObjects mo, int sensorRange) {
		super(position, name, mo, sensorRange);
		this.pathfinder = pathfinder;
		this.goal = goal;
		roboLogger.path.add(getCurrentPosition());
	}
	
	public DStarRobot(Vector3d position, String name, PathAlgorithm pathfinder,
			Point2d goal, MovingObjects mo, int sensorRange, int concaveRange) {
		super(position, name, mo, sensorRange,true,concaveRange);
		this.pathfinder = pathfinder;
		this.goal = goal;
		roboLogger.path.add(getCurrentPosition());
		
	}

	@Override
	public void performBehavior() {
				
		if(!isFinished()) {
			
	       	//This is initBehavior()
			updateRoboMap();
	    	if(path == null) {
	            this.path = pathfinder.getPath(this.roboMap, this.getCurrentPosition(), this.goal, getLifeTime());
	            if(path != null) {
	            	roboLogger.pathsCalculated++;
	            }
	            this.setIndex(0);
	            if(path != null) {
	            	rotateTowardsNextGoal(path[getIndex()]);
	            	setTranslationalVelocity(1);
	            }
	    	}
	    	
	    	
	    	
	    	if( sensors.getMeasurement(0) < 0.4 || 
	 	        sensors.getMeasurement((int)(0.125*sensors.getNumSensors())) < 0.4 ||
	 	        sensors.getMeasurement((int)(0.875*sensors.getNumSensors())) < 0.4) {
	    		updateRoboMap();
	    		this.path = pathfinder.getPath(roboMap, getCurrentPosition(), goal, getLifeTime());
	    		if(path != null) {
	            	roboLogger.pathsCalculated++;
	            }
	    		setIndex(0);
	    	}
	    	    	
	    	if(path !=null && getIndex() < this.path.length 
	    			&& this.getCurrentPosition().distance(this.path[getIndex()]) < ACCEPT_DISTANCE) {
	    		
	    		setTranslationalVelocity(0);
	    		roboLogger.path.add(path[getIndex()]);
	    		setIndex(getIndex()+1);
	    		if(getIndex() < this.path.length) {
	    			
	    			boolean objectInPath = objectConflict(getCurrentPosition(), path[getIndex()]);
	    			
	    			if(!objectInPath) {
	    				for(int i = getIndex(); i < path.length-1; i++) {
	    					objectInPath = objectConflict(path[i], path[i+1]);
	    					if(objectInPath) {
	    						break;
	    					}
	    				}
	    			}
	    			
	    			if (objectInPath) {
	    				//Redo path
	    				path = pathfinder.getPath(roboMap, getCurrentPosition(), goal, getLifeTime());
	    				if(path != null) {
	    	            	roboLogger.pathsCalculated++;
	    	            }
	    				setIndex(0);   						
	    			}
	    			if(path != null) {
	    				rotateTowardsNextGoal(this.path[getIndex()]);
	    				setTranslationalVelocity(1);
	    			}
	    			
	    		}
	    	}
	    	
		}

		if(this.path == null) {
			if(!isFinished()) {
				finish("No path to goal... :'(");
			}
			if(rotate == 5) { 
				rotate = 0;
				rotateDir *= -1;
			}
			rotateY(rotateDir);
			rotate++;
			
			return;
		}
		
		else if(this.getCurrentPosition().distance(goal) < ACCEPT_DISTANCE) {
			
			if(!isFinished()) {
				finish("Reached goal! :)");
			}
			rotateY(0.1f);
			return;
		}
		
	}
	
	private void finish(String message) {
		roboLogger.distanceTraveled = getOdometer();
		System.out.println(message);
		System.out.println("------------------");
		System.out.println("------------------");
		printStatistics();
		setFinished(true);
		setTranslationalVelocity(0);
		Main.finishRun(roboLogger, "DStar Robot "+getName()+", "+message);
	}
	
	private int rotate = 0;
	private float rotateDir = 0.1f;
	
    private boolean objectConflict(Point2d from, Point2d to) {
    	
    	if(roboMap.contain(RoboMap.Content.OBJECT, to)) {
    		return true;
    	}
    	
    	
    	if(from.x != to.x && from.y != to.y) {
    		
    		int x = from.x < to.x ? 1:-1;
        	int y = from.y < to.y ? 1:-1;
        	
   
        	return roboMap.contain(RoboMap.Content.OBJECT, new Point2d(from.x+x,from.y)) ||
        		   roboMap.contain(RoboMap.Content.OBJECT, new Point2d(from.x,from.y+y));
    	}
    	return false;
    }

}
